#ifndef GRIPPER_H
#define GRIPPER_H

#include <atomic>
#include <pthread.h>
#include <vector>
#include <memory> // 添加头文件
#include <modbus/modbus-rtu.h>

namespace DriverSDK {

constexpr uint16_t REG_ENABLE = 0x0100;
constexpr uint16_t REG_POS_LOW = 0x0102;
constexpr uint16_t REG_SPEED = 0x0104;
constexpr uint16_t REG_TORQUE = 0x0105;
constexpr uint16_t REG_TRIGGER = 0x0108;
constexpr uint16_t REG_REALTIME_POS_LOW = 0x0609;
constexpr uint16_t REG_POS_REACHED = 0x0602;
constexpr uint16_t REG_TORQUE_REACHED = 0x0601;
constexpr uint16_t REG_SPEED_REACHED = 0x0603;

struct GripperTarget {
    int32_t position;  // 位置
    uint16_t speed;    // 速度
    uint16_t torque;   // 扭矩
};

struct GripperStatus {
    int32_t position;     // 当前夹爪位置
    uint16_t pos_reached; // 位置是否到达标记
};

// Gripper 类，表示一个夹爪
class Gripper {
public:
   Gripper(const char* port, int slave_id);
    ~Gripper();
    Gripper(const Gripper&) = delete;
    Gripper& operator=(const Gripper&) = delete;

    void setTarget(const GripperTarget& target);  // 设置目标
    void updateStatus();  // 更新夹爪状态
    void getStatus(GripperStatus* status);  // 获取状态
    bool control();  // 控制夹爪的开合

private:
    modbus_t* connect(const char* port, int slave_id);  // 建立连接
    bool writePosition(int32_t position);  // 写入夹爪位置
    // void updateStatus();  // 更新夹爪状态
    bool modbusWrite(uint16_t reg, uint16_t value);

    modbus_t* mb;
    pthread_mutex_t mutex_;
    std::atomic<bool> running_;
    int slave_id_;
    GripperTarget target_;
    GripperStatus status_;
};

class GripperController {
public:
    GripperController(){

    };
    ~GripperController(){

    };

    void addGripper(const char* port, int slave_id);  // 添加夹爪
    int run();  // 启动控制线程
    void stop();  // 停止控制线程
    void setTarget(int index, const GripperTarget& target);  // 设置夹爪目标
    void getStatus(int index, GripperStatus* status);  // 获取夹爪状态

private:
    static void* controlThread(void* arg);  // 控制线程的入口
    void controlLoop();  // 控制循环
    std::vector<std::unique_ptr<Gripper>> grippers_;     
    std::atomic<bool> running_;
    pthread_t pth_;
};

} // namespace DriverSDK

#endif // GRIPPER_H

